import numpy as np

intrinsics = np.asarray([[976.96691895, 0., 0.],
                         [0., 976.96691895, 0., ],
                         [721.31268311, 536.68408203, 1., ]])

txt_path = 'resources/pj.txt'
M_proj = np.loadtxt(txt_path, dtype=np.float32)

print(intrinsics)
in_fx = intrinsics[0, 0]
in_fy = intrinsics[1, 1]
in_cx = intrinsics[2, 0]
in_cy = intrinsics[2, 1]

print(in_fx, in_fy, in_cx, in_cy)

print(in_fx / in_cx)
print(in_fy / in_cy)

print(M_proj)

# print(M_proj)


affine = np.asarray([[3.29896907e-01, 0.00000000e+00, -5.69402062e+02],
                     [0.00000000e+00, 3.29896907e-01, -4.06762887e+02, ],
                     [0, 0, 1]])

print(affine)

c_af = affine @ intrinsics.T

c_af = c_af.T

print(c_af)

# new_pjm = np.asarray([[1.57443701, 0.0, 0.0, 0.0, ],
#                       [0.0, 1.57443701, 0.0, 0.0, ],
#                       [0.0, 0.0, -0.99999976, -1.0, ],
#                       [0.0, 0.0, -0.001, 0.0]])
# new_pjm[0, 0] = c_af[0, 0] / c_af[2, 0]
# new_pjm[1, 1] = c_af[1, 1] / c_af[2, 1]
#
# print(new_pjm)
